/**
 * 
 */
package ipc;

import graph.Graph;

import java.io.File;
import java.io.IOException;
import java.math.BigDecimal;

import rddl.RDDL.KronDelta;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Iterator;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.TreeMap;
import java.util.TreeSet;

import logic.add.AADD;
import logic.add.AADDINode;
import logic.add.AADDRNode;
import logic.add.ADDDNode;
import logic.add.ADDNode;
import logic.add.ADDRNode;

import dd.discrete.DD;

import rddl.EvalException;
import rddl.RDDL;
import rddl.RDDL.AGG_EXPR;
import rddl.RDDL.BOOL_CONST_EXPR;
import rddl.RDDL.BOOL_EXPR;
import rddl.RDDL.Bernoulli;
import rddl.RDDL.COMP_EXPR;
import rddl.RDDL.CPF_DEF;
import rddl.RDDL.DOMAIN;
import rddl.RDDL.DiracDelta;
import rddl.RDDL.EXPR;
import rddl.RDDL.IF_EXPR;
import rddl.RDDL.INSTANCE;
import rddl.RDDL.INT_CONST_EXPR;
import rddl.RDDL.LCONST;
import rddl.RDDL.LTERM;
import rddl.RDDL.LTYPED_VAR;
import rddl.RDDL.LVAR;
import rddl.RDDL.NONFLUENTS;
import rddl.RDDL.OPER_EXPR;
import rddl.RDDL.PVARIABLE_DEF;
import rddl.RDDL.PVAR_EXPR;
import rddl.RDDL.PVAR_INST_DEF;
import rddl.RDDL.PVAR_NAME;
import rddl.RDDL.QUANT_EXPR;
import rddl.RDDL.TYPE_NAME;
import rddl.State;
import rddl.competition.Client;
import rddl.parser.ParseException;
import rddl.parser.parser;
import rddl.policy.Policy;
import rddl.viz.ElevatorDisplay;
import rddl.viz.RDDL2Graph;
import util.Pair;

import rddl.RDDL.REAL_CONST_EXPR;
/**
 * @author NR
 *
 * TODO
 */
public class ValueIterationConcurrentAADD {

	public State      _state;
	public INSTANCE   _i;
	public NONFLUENTS _n;
	public DOMAIN     _d;
	
	public static double EPSILON = 0.1;
	
	public static final int ORDER_XAXP = 1;
	public static final int ORDER_AXXP = 2;
	public static final int ORDER_XPAX = 3;
	public static final int ORDER_XXPA = 4;
	public static final int ORDER_AXPX = 5;
	public static final int ORDER_XPXA = 6;
	public static final int ORDER_GUESS = 7;
	private static final int ORDER_INTERLEAVE = 8;
	
	public static final double BIGADD = 1e5;	

	private int _order;
	private ArrayList<String> _alOrder;
	
	public AADD         _context;
	public ArrayList<Integer> _alSaveNodes;
	public int         DD_ZERO;
	public int         DD_ONE;

	public RDDL2Graph _r2g;
	public String _filename;
	
	public TreeMap<String, Pair<PVAR_NAME, ArrayList<LCONST> > > _hmStateVars;
	public TreeMap<String, Pair<PVAR_NAME, ArrayList<LCONST> > > _hmNextStateVars;
	public TreeMap<String, Pair<PVAR_NAME, ArrayList<LCONST> > > _hmActionMap;
	public TreeMap<String, Pair<PVAR_NAME, ArrayList<LCONST> > > _hmObservVars;
	public HashMap<Integer, Integer> _hmPrimeRemap;
	
	
	public HashMap<String , Integer > _hmVar2ID = new HashMap<String, Integer>();
	public HashMap<Integer, String > _hmID2Var = new HashMap<Integer, String>();
	
	public TreeMap<String, Integer> _var2transDD;
	public TreeMap<String, Integer> _var2observDD;
	
	public final static int     STATE_ITER  = 0;
	public final static int     OBSERV_ITER = 1;
	private static final int BASE_ORDERID = 216;
	
	private int _rewardDD;
	private int _valueDD;
	
	private Map<String, List<String> > _hmact2vars = new HashMap<String, List<String> >();
	private Policy policy;
	
	public List<iterationData> iData = new ArrayList<iterationData>(); // :-)
	public preSolve presolve;
	
	public double avgR;
	public double varR;
	
	public long timetoplay;
	public postSolve postsolve;

	public runInfo rInfo;

	private TreeSet<Pair<Integer, Integer>> _hsPairConstraints = new TreeSet<Pair<Integer,Integer>>();
	private ArrayList<String> _alactionvars = new ArrayList<String>();
	private ArrayList<String> _alstatevars = new ArrayList<String>();
	private ArrayList<String> _alnextstatevars = new ArrayList<String>();
	private ArrayList<String> _alObservvars = new ArrayList<String>();
	
	private static final boolean	DEBUG0	= true;
	private static final boolean	DEBUG2	= false;
	private static final boolean	DEBUG4	= false;
	private static final Runtime	RUNTIME	=  Runtime.getRuntime() ;
	
	public ValueIterationConcurrentAADD( final String domain_file, final String instance_file, 
									 final Double epsilon, final Integer order, final Integer degree,
									 final String dest_dir) {
			
			try
			{
				_order = order;
				EPSILON = epsilon;
				
				RDDL _rddltemp = new RDDL();
				
				File domf = new File(domain_file);
				File instf = new File(instance_file);
				
				
				_rddltemp.addOtherRDDL(parser.parse(domf));
				_rddltemp.addOtherRDDL(parser.parse(instf));
				
				doVI(_rddltemp, degree, dest_dir);
			}catch(Exception e){
				e.printStackTrace();
			}
	}
	
	public ValueIterationConcurrentAADD(RDDL rddl, final double epsilon, final int order, final int degree,
			 final String dest_dir) throws Exception{
		_order = order;
		EPSILON = epsilon;
		doVI(rddl, degree, dest_dir);
	}
	
	public void doVI(RDDL _rddltemp, final int degree, final String dest_dir) throws Exception{
				
				this._d= _rddltemp._tmDomainNodes.entrySet().iterator().next().getValue();
				this._i= _rddltemp._tmInstanceNodes.entrySet().iterator().next().getValue();
				this._n= _rddltemp._tmNonFluentNodes.entrySet().iterator().next().getValue();
				
				if( degree != -1){
					_i._nNonDefActions = degree;
				}
				
				rInfo = new runInfo("AADD-CONC", _i._sName, _i._nNonDefActions, _d._sDomainName);
				
				_state = new State();
				
				_state.init(_n != null ? _n._hmObjects : null, _i._hmObjects,  
						_d._hmTypes, _d._hmPVariables, _d._hmCPF,
						_i._alInitState, _n == null ? null : _n._alNonFluents, 
						_d._alStateConstraints, _d._exprReward, _i._nNonDefActions);
				
				//for each ground state fluent
				//	get corresponding cpf EXPR
				//	ground it/ eliminate non fluents/ quantifiers
				//	build ADD
				//	show graph
				long _time = System.currentTimeMillis();
				buildCPTs();
				
				presolve = new preSolve( getSizeofR(), getSizeofP(), getSizeofA(), (System.currentTimeMillis()-_time)/(1000d*60));
				System.out.println(presolve);
				System.out.println(rInfo);
			


				ArrayList<String> _alElimOrder = new ArrayList<String>();
				ArrayList<String> _alSumOrder = new ArrayList<String>(_hmStateVars.keySet());
				makeOrders(_alSumOrder, _alElimOrder);

				
				//value iteration begin
				_valueDD = DD_ZERO;
				
				int Vprev;
				
				boolean done = false;
				int iter = 0;
				
				_time = System.currentTimeMillis();
				
				while( !done ) {
					
					long itertime = System.currentTimeMillis();
					
					Vprev = _valueDD;
					
//					convert to primed diagram
					//x -> x' action vars not changed
					int Vprimed = _context.remapGIDsInt(Vprev, _hmPrimeRemap);
					Vprimed = _context.align(Vprimed);
					if( !_context.verifyOrder(Vprimed) ) {
						System.err.println("V' violates order");
						System.exit(1);
					}
					
					if( DEBUG4 ) {
						System.out.println("Displaying V'");
						_context.getGraph(Vprimed, _hmID2Var).launchViewer();
						System.out.println("Press key");
						System.in.read();
					}
					
					//SANITY Check : Vprimed does not contain unprimed variables
					if( DEBUG0 ) {
						Set nodes = _context.getGIDs(Vprimed);
						for( String s : _alstatevars ) {
							if( nodes.contains(_hmVar2ID.get(s)) ) {
								System.err.println("V' contains x");
								System.exit(1);
							}
						}
						for( String a : _alactionvars ) {
							if( nodes.contains(_hmVar2ID.get(a)) ) {
								System.err.println("V' contains a");
								System.exit(1);
							}
						}
					}
					
					Vprimed = applyZeroOneConstraints(Vprimed, Vprev);
					
					if( DEBUG2 ) {
						System.out.println("size of V' after constraint : " + _context.countExactNodes(Vprimed));	
					}
					
					if( DEBUG4 ) {
						System.out.println("size of V' after constraint : " + _context.countExactNodes(Vprimed));
						System.out.println("Displaying V' after zero constraints");
						_context.getGraph(Vprimed, _hmID2Var).launchViewer();
						System.out.println("Press key");
						System.in.read();
					}
					
					Set gids = _context.getGIDs(Vprimed);
					
					for( String var : _alSumOrder ){
			
						String primedvar = var;
						int primedvarid = (Integer)(_hmVar2ID.get(primedvar));
						
						if( ! gids.contains(primedvarid) ){
							if( DEBUG2 ) {
								System.out.println("Skipped " + primedvar);	
							}
							continue;
						}
						
						int dualdd = _var2transDD.get(primedvar);
						
						if( DEBUG2 ) {
							System.out.println("Multiplying " + primedvar);
						}
						Vprimed = _context.applyInt(dualdd, Vprimed, DD.ARITH_PROD);
						
						if( DEBUG2 ) {
							System.out.println("Summing " + primedvar);
						}
						Vprimed = _context.opOut(Vprimed, primedvarid, DD.ARITH_SUM);
						
						if( DEBUG2 ) {
							System.out.println("Expectation " + primedvar);
							System.out.println(" size: " + _context.countExactNodes(Vprimed));
							System.out.println(showMemory());
						}
	
						flushCaches(Vprimed, Vprev);
//						while(size >= BIGADD){
//							System.out.println("Size before prune : " + size + " " + precision );
//							_context.PRUNE_TYPE = DD.REPLACE_RANGE;
//							_context.PRUNE_PRECISION = precision;
//							Vprimed = _context.pruneNodes(Vprimed);
//							size = _context.countExactNodes(Vprimed);
//							System.out.println("Size after prune : " + size);
//							precision *= 10;
//							flushCaches(Vprimed, Vprev);
//	
//						}
				

					}
					
					if( DEBUG0 ) {
						Set nodes = _context.getGIDs(Vprimed);
						for( String ns : _alnextstatevars ) {
							if( nodes.contains(_hmVar2ID.get(ns) ) ) {
								System.err.println("EV' contains x'");
								System.err.println("Displaying V'");
								_context.getGraph(Vprimed, _hmID2Var).launchViewer();
								System.out.println("Press key");
								System.in.read();
								System.exit(1);
							}
						}
					}

					Vprimed = _context.scalarMultiply(Vprimed, _i._dDiscount);
					
					if( DEBUG2 ) {
						System.out.println("Size before making negative infs. to EV'");
						System.out.println(_context.countExactNodes(Vprimed));
					}
					
					Vprimed = applyConstraintsInterleaved(Vprimed, Vprev);
					
					if( DEBUG2 ) {
						System.out.println("Size after making negative infs. to EV'");
						System.out.println(_context.countExactNodes(Vprimed));
					}
					
					if( DEBUG2 ) {
						System.out.println("Adding reward");
					}

					_valueDD = _context.applyInt(Vprimed, _rewardDD, DD.ARITH_SUM);
						
					if( DEBUG2 ) {
						System.out.println("Size of V before constraints " + _context.countExactNodes(_valueDD));
						System.out.println(showMemory());
					}

//					_valueDD = applyConstraintsInterleaved(_valueDD, Vprev);
					
					int savedd = _valueDD;
					long sizeofq = 0;
					
					if( DEBUG2 ) {
						sizeofq = _context.countExactNodes(savedd);
					}
					
					if( DEBUG2 ) {
						System.out.println("Size of V after constraints " + _context.countExactNodes(_valueDD));
						System.out.println(showMemory());
					}
					
					
					if( DEBUG2 ) {
						System.out.println("maxing out actions");
					}
						
					_valueDD = maxOutActionVars(_valueDD, _alElimOrder, Vprev);
							
					if( DEBUG2 ) {
						System.out.println("Size after maxing actions " + _context.countExactNodes(_valueDD));
						System.out.println(showMemory());
					}

					if( DEBUG4 ) {
						System.out.println("Displaying V" + iter);
						_context.getGraph(_valueDD, _hmID2Var).launchViewer();
						System.out.println("Press key");
						System.in.read();
					}

					int diff  = _context.applyInt(_valueDD, Vprev, DD.ARITH_MINUS);
					double max_pos_diff = _context.getMaxValue(diff);
                    double max_neg_diff = _context.getMinValue(diff);
                    double error = Math.max(Math.abs(max_pos_diff), Math.abs(max_neg_diff));

					++iter;
					
					System.out.println("iteration " + iter + " " + error );
					
					iData.add( new iterationData(iter, getSizeofV(), 0, (System.currentTimeMillis()-itertime)/(1000d*60), error, sizeofq ) );
					
					
					
					if( iter == _i._nHorizon || error <= EPSILON ){
						done = true;
						sizeofq = _context.countExactNodes(savedd);
						_valueDD = savedd;
					}
					
					
					if( done ){
						flushCaches(savedd);
					}else{
						flushCaches();
					}
					
				}
				
				long timetosolve = System.currentTimeMillis()-_time;
				
				playDD();
				
				postsolve = new postSolve(avgR, varR, getSizeofV(), 0, timetoplay/(1000d*60), timetosolve/(1000d*60));
				
				
	}

	public String showMemory() {
		
	}
	
	private double extent(int dd) {
		
		ADDNode root = _context.getNode(dd);
		if( root instanceof ADDDNode ){
			return 0;
		}else{
			AADDINode node = (AADDINode)root; 
			return node._dMaxLower-node._dMinLower;
		}
		
	}
	


	private long getSizeofV() {
		return _context.countExactNodes(_valueDD);
	}

	private long getSizeofR() {
		return _context.countExactNodes(_rewardDD);
	}

	private long getSizeofA() {
		return _hmActionMap.size();
	}

	private long getSizeofP() {
		long ret = 0;
		
		for( Integer i : _var2transDD.values() ){
			ret += _context.countExactNodes(i);
		}
		
		return ret;
	}
	
	private void playDD() {
		
		policy = new Policy() {
		
			public double avg_reward = 0 ;
			public double variance = 0;//sum of squares till done
			public int horizon;
			public int numRounds = 30;
			private long ptime;
			ElevatorDisplay disp = new ElevatorDisplay();
			
			
			@Override
			public void roundEnd(double reward) {
//				super.roundEnd(reward);
				avg_reward += reward;
				variance += (reward*reward);
			}
			
			@Override
			public void sessionEnd(double total_reward) {
//				super.sessionEnd(total_reward);
				avg_reward /= numRounds;
				variance -= numRounds*(avg_reward*avg_reward);
				variance /= (numRounds-1);
				variance = Math.sqrt(variance);
//				System.out.println("Average reqward : " + avg_reward);
//				System.out.println("Stdev: " + variance);
				avgR = avg_reward;
				varR = variance;
				timetoplay = System.currentTimeMillis()-ptime;
			}
			
			@Override
			public void roundInit(double time_left, int horizon, int round_number, int total_rounds) {
//				super.roundInit(time_left, horizon, round_number, total_rounds);
				this.horizon = horizon;
				this.ptime = System.currentTimeMillis();
			}
			
			
			
			@Override
			public ArrayList<PVAR_INST_DEF> getActions(State s) throws EvalException {
//				disp.getStateDescription(s);
				
				ArrayList<PVAR_INST_DEF> ret = new ArrayList<PVAR_INST_DEF>();
				
				int simplifieddd = _valueDD;
				

				for( Map.Entry<String, Pair<PVAR_NAME, ArrayList<LCONST>>> entry : _hmStateVars.entrySet() ){
					String name = entry.getKey();
					Pair<PVAR_NAME, ArrayList<LCONST>> statevar = entry.getValue();
					
					Boolean val = (Boolean)s.getPVariableAssign(statevar._o1, statevar._o2);
//					System.out.println("State :  " + name + " : " + val );
					
					int nodeid = (Integer)_hmVar2ID.get(name);
					int op = val ? DD.RESTRICT_HIGH : DD.RESTRICT_LOW;
					simplifieddd  = _context.restrict(simplifieddd,	nodeid, op);
					
				}
					
				while( true ){
					
					AADDRNode nod = _context.getRNode(simplifieddd);
					
					ADDNode inner = _context.getNode(nod._nRefID);
					
					if( inner instanceof ADDDNode ){
//						System.out.println("Hit leaf value : " + ((ADDDNode)nod)._dUpper);
						break;
					}else{
						AADDINode n = (AADDINode)inner;
						
						double max = n._dMaxUpper;
						
						int testvarid = n._nGlobalID;
						
						String testvarname = (String)_hmID2Var.get(testvarid);
						
						Pair<PVAR_NAME, ArrayList<LCONST>> act = _hmActionMap.get(testvarname);
						
						Boolean defaultval = (Boolean) s.getDefaultValue(act._o1);
						
						ADDNode truthnode = _context.getNode(n._nHigh);
						
						ADDNode falsenode = _context.getNode(n._nLow);
						
						double maxtrue, maxfalse;
						
						if( truthnode instanceof AADDINode ){
							maxtrue = n._dHighMult * ((AADDINode)truthnode)._dMaxUpper + n._dHighOffset;
						}else{
							maxtrue = n._dHighMult * ((ADDDNode)truthnode)._dUpper + n._dHighOffset;
						}
						
						if( falsenode instanceof AADDINode ){
							maxfalse = n._dLowMult * ((AADDINode)falsenode)._dMaxUpper + n._dLowOffset;
						}else{
							maxfalse = n._dLowMult * ((ADDDNode)falsenode)._dUpper + n._dLowOffset;
						}
						
//						System.out.println(max + " " + maxfalse + " " + maxtrue );
						
						if( max == maxfalse ){
							if( defaultval != false ){
								ret.add(new PVAR_INST_DEF(act._o1.toString(), false, act._o2));
							}
							simplifieddd = _context.restrict(simplifieddd, testvarid, DD.RESTRICT_LOW);
						}else if( max == maxtrue ){
							if( defaultval != true ){
								ret.add(new PVAR_INST_DEF(act._o1.toString(), true, act._o2));
							}
							simplifieddd = _context.restrict(simplifieddd, testvarid, DD.RESTRICT_HIGH);
						}
						
					}
				}
				
				return ret;
			}
		
			public double getAvg_reward() {
				return avg_reward;
			}
			
			public double getVariance() {
				return variance;
			}
			
		};
		
		Client cl = new Client(this._d, this._i, this._n, policy, "localhost", 2316, "BEAVER!!!");
	}
	
	private int applyConstraintsInterleaved(int dd, int... save){
		int ret = dd;
		Iterator<Pair<Integer, Integer>> it = _hsPairConstraints.iterator();
		
		int[] saved = new int[save.length+1];
		for( int i = 0 ; i < save.length; ++i ){
			saved[i] = save[i];
		}
		
		while( it.hasNext() ){
			Pair<Integer, Integer> con = it.next();
			ret = _context.applyInt(ret, con._o1,	DD.ARITH_PROD);
			ret = _context.applyInt(ret, con._o2, DD.ARITH_SUM);
			saved[save.length] = ret;
			flushCaches(saved);
		}
		
		return ret;
		
	}
	
	private int applyNegInfConstraints(int dd, int... save) {
		int ret = dd ; 
		
		int[] saved = new int[save.length+1];
		for( int i = 0 ; i < save.length; ++i ){
			saved[i] = save[i];
		}
		
		Iterator<Pair<Integer, Integer>> it = _hsPairConstraints.iterator();
		
		while( it.hasNext() ){
			ret = _context.applyInt(ret, it.next()._o2, DD.ARITH_SUM);
			saved[save.length] = ret;
			flushCaches(saved);
		}
		
		return ret;
		
	}

	private int applyZeroOneConstraints(int dd, int... save) {
		int ret = dd ; 
		
		int[] saved = new int[save.length+1];
		for( int i = 0 ; i < save.length; ++i ){
			saved[i] = save[i];
		}
		
		Iterator<Pair<Integer, Integer>> it = _hsPairConstraints.iterator();
		
		while( it.hasNext() ){
			ret = _context.applyInt(ret, it.next()._o1, DD.ARITH_PROD);
			saved[save.length] = ret;
			flushCaches(saved);
		}
		
		return ret;
		
	}

	private int maxOutActionVars(int dd, ArrayList<String> _alElimOrder, int... save) {
		
		int ret = dd ;
		int[] saved = new int[save.length+1];
		for( int i = 0 ; i < save.length; ++i ){
			saved[i] = save[i];
		}
		
		for( String act : _alElimOrder ){
			int actid = (Integer)_hmVar2ID.get(act);
			ret = _context.opOut(ret, actid, DD.ARITH_MAX);
			saved[saved.length-1] = ret;
			flushCaches(saved);
		//	System.out.println(_context.countExactNodes(ret));
		}
		
		return ret;
		
	}

	public void flushCaches(int... save){
		_context.clearSpecialNodes();
		_context.addSpecialNode(_rewardDD);
		_context.addSpecialNode(_valueDD);
		for( int special : _alSaveNodes ){
			_context.addSpecialNode(special);
		}
		
		for( Pair<Integer,Integer> special : _hsPairConstraints ){
			_context.addSpecialNode(special._o1);
			_context.addSpecialNode(special._o2);
		}
		
		for( int special : save ){
			_context.addSpecialNode(special);
		}
			
		_context.flushCaches(false);
	}
	
	public void buildCPTs() throws Exception {

		_var2transDD = new TreeMap<String,Integer>();
		_var2observDD = new TreeMap<String,Integer>();
		
		// Verify no intermediate variables
		if (_state._tmIntermNames.size() > 0)
			throw new Exception("Cannot convert to SPUDD format: contains intermediate variables");

		// Get all variables
		TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> state_vars  = collectStateVars();
		TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> action_vars = collectActionVars();
		TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> observ_vars = collectObservationVars();
		
		_hmStateVars = new TreeMap<String, Pair<PVAR_NAME,ArrayList<LCONST>>>();
		_hmNextStateVars = new TreeMap<String, Pair<PVAR_NAME,ArrayList<LCONST>>>();
		_hmActionMap = new TreeMap<String, Pair<PVAR_NAME,ArrayList<LCONST>>>();
		_hmObservVars = new TreeMap<String, Pair<PVAR_NAME,ArrayList<LCONST>>>();
		
		for (Map.Entry<PVAR_NAME,ArrayList<ArrayList<LCONST>>> e : state_vars.entrySet()) {
			PVAR_NAME p = e.getKey();
			ArrayList<ArrayList<LCONST>> assignments = e.getValue();
			for (ArrayList<LCONST> assign : assignments) {
				String name = CleanFluentName(p.toString() + assign);
				_hmStateVars.put(name, new Pair(p, assign));
				_alstatevars.add(name);
				_hmNextStateVars.put(name + "'", new Pair(p, assign));
				_alnextstatevars.add(name+"'");
			}
		}

		
		for (Map.Entry<PVAR_NAME,ArrayList<ArrayList<LCONST>>> e : action_vars.entrySet()) {
			PVAR_NAME p = e.getKey();
			ArrayList<ArrayList<LCONST>> assignments = e.getValue();
			for (ArrayList<LCONST> assign : assignments) {
				String name = CleanFluentName(p.toString() + assign);
				_hmActionMap.put(name, new Pair(p, assign));
				_alactionvars.add(name);
			}
		}

		 
		
		for (Map.Entry<PVAR_NAME,ArrayList<ArrayList<LCONST>>> e : observ_vars.entrySet()) {
			PVAR_NAME p = e.getKey();
			ArrayList<ArrayList<LCONST>> assignments = e.getValue();
			for (ArrayList<LCONST> assign : assignments) {
				String name = CleanFluentName(p.toString() + assign);
				_hmObservVars.put(name + "'", new Pair(p, assign));
				_alObservvars.add(name+"'");
				// SPUDD uses primed observation vars
			}
		}

		_alOrder = new ArrayList<String>();
		
		if( _order != ORDER_GUESS ){
			_alOrder = makeOrdering();
		}else{
			_alOrder = guessOrdering(state_vars, observ_vars, action_vars);
		}
		
		int orderid = BASE_ORDERID;
		ArrayList<Integer> _alIntOrder = new ArrayList<Integer>();
		
		for( String var : _alOrder ){
//			System.out.println(var);
			Integer id = _hmVar2ID.get(var);
			if( id == null ){
				_hmVar2ID.put(var, orderid );
				_hmID2Var.put(orderid, var);
				_alIntOrder.add(orderid);
				++orderid;
			}else{
				_alIntOrder.add(id);
			}
		}
		
		_hmPrimeRemap = new HashMap<Integer,Integer>();
		
		for( Map.Entry<String, Pair<PVAR_NAME,ArrayList<LCONST>>> entry : _hmStateVars.entrySet() ){
			String name = entry.getKey();
			_hmPrimeRemap.put(_hmVar2ID.get(name), _hmVar2ID.get(name+"'"));
		}
		
		_context = new AADD(_alIntOrder);

		DD_ONE = _context.getConstantNode(1d);
		DD_ZERO = _context.getConstantNode(0d);
		_valueDD = DD_ZERO;
		_rewardDD = DD_ZERO;
		
		_alSaveNodes = new ArrayList<Integer>();
		_alSaveNodes.add(DD_ONE);
		_alSaveNodes.add(DD_ZERO);
		
		if( DEBUG4 ) {
			System.out.println("State vars:  " + state_vars);
			System.out.println("Action vars: " + action_vars);
			System.out.println("Observ vars: " + observ_vars);	
		}

		
		for (int iter = STATE_ITER; iter <= OBSERV_ITER; iter++) {
			
			TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> src = 
				iter == STATE_ITER ? state_vars : observ_vars;
			
			for (Map.Entry<PVAR_NAME,ArrayList<ArrayList<LCONST>>> e : src.entrySet()) {
				
				// Go through all variable names p for a variable type
				PVAR_NAME p = e.getKey();
				ArrayList<ArrayList<LCONST>> assignments = e.getValue();
				//System.out.println(_state._hmCPFs);
				
				CPF_DEF cpf = _state._hmCPFs.get(new PVAR_NAME(p.toString() + 
						(iter == STATE_ITER ? "'" : "")));
				
				
				HashMap<LVAR,LCONST> subs = new HashMap<LVAR,LCONST>();
				for (ArrayList<LCONST> assign : assignments) {
	
					String cpt_var = CleanFluentName(p.toString() + assign);
//					System.out.println("Processing: " + cpt_var);
	
					subs.clear();
					for (int i = 0; i < cpf._exprVarName._alTerms.size(); i++) {
						LVAR v = (LVAR)cpf._exprVarName._alTerms.get(i);
						LCONST c = (LCONST)assign.get(i);
						subs.put(v,c);
					}
					
					// This method determines all ground fluents relevant to 
					// the cpf for a variable for the given substitution 'subs'
					HashSet<Pair> relevant_vars = new HashSet<Pair>();
					EXPR cpf_expr = cpf._exprEquals;
					if (_d._bCPFDeterministic) // collectGFluents expects a distribution so convert to a Delta function if needed
						cpf_expr = new KronDelta(cpf_expr);					
					
					cpf_expr.collectGFluents(subs, _state, relevant_vars);
					
					for (Map.Entry<PVAR_NAME,ArrayList<ArrayList<LCONST>>> e2 : action_vars.entrySet()) {
	
						PVAR_NAME action_name = e2.getKey();
						ArrayList<ArrayList<LCONST>> action_assignments = e2.getValue();
						Boolean defaultval = (Boolean)_state.getDefaultValue(action_name);
						
						// Go through instances for action_name
						for (ArrayList<LCONST> action_assign : action_assignments)
							_state.setPVariableAssign(action_name, action_assign, defaultval);
	
					}

					// CONCURRENT CASE, NO NEED TO ENUMERATE ACTIONS (IMPLICIT VARS IN CPT)
					int cpt = enumerateAssignments(new ArrayList<Pair>(relevant_vars), cpf_expr, subs, 0);
					
					//build dual diagram
					int unprime_id = (Integer)_hmVar2ID.get(cpt_var);
					int prime_id = (Integer)_hmVar2ID.get(cpt_var+"'");
						
					int prob_true = cpt;
						
					int prob_false = _context.applyInt(DD_ONE, prob_true, DD.ARITH_MINUS);
						
					int high_branch = _context.getVarNode(prime_id, 0.0d, 1.0d);
						
					int low_branch = _context.getVarNode(prime_id, 1.0d, 0.0d);
						
					int low = _context.applyInt(low_branch, prob_false, DD.ARITH_PROD);
						
					int high = _context.applyInt(high_branch, prob_true, DD.ARITH_PROD);
						
					cpt = _context.applyInt(high, low,DD.ARITH_SUM);
					
					_alSaveNodes.add(cpt);
					
					if (iter == STATE_ITER)
						_var2transDD.put(cpt_var+"'", cpt);
					else
						_var2observDD.put( cpt_var+"'", cpt);
					
					if( DEBUG4 ) {
						System.out.println("Displaying CPT for " + cpt_var+"'");
						_context.getGraph(cpt, _hmID2Var).launchViewer();
						System.out.println("Press any key");
						System.in.read();
					}
				}
			}
		}
		
		getConcurrenyDD(action_vars);
		
		for( EXPR constraint : _state._alConstraints ){
//			System.out.println("Constraint : " + constraint ); 
			if( constraint instanceof BOOL_EXPR ){
				BOOL_EXPR be = (BOOL_EXPR)constraint;
				addConstraint(be);
				
			}else{
				System.err.println("Constraint not tpye of bool expr");
				System.exit(1);
			}
		}
		
		HashSet<Pair> relevant_vars = new HashSet<Pair>();
		
		EXPR rew_expr =  _state._reward;
//		if (_d._bRewardDeterministic) // collectGFluents expects distribution
//			rew_expr = new DiracDelta(rew_expr);

		try {
			ArrayList<Integer> rew = convertAddExpr2ADD(rew_expr, false);
			
			_rewardDD = DD_ZERO;
			for( int subd : rew ){
				_rewardDD = _context.applyInt(_rewardDD, subd, DD.ARITH_SUM);
			}
			
			double rmin = Math.abs(_context.getMin(_rewardDD));

			int shiftdd = _context.getConstantNode(1+rmin);
			_rewardDD = _context.applyInt(_rewardDD, shiftdd, DD.ARITH_SUM );
			_rewardDD = applyConstraintsInterleaved(_rewardDD);
			_context.addSpecialNode(_rewardDD);
			
			if( DEBUG4 ) {
				System.out.println("Displaying reward");
				_context.getGraph(_rewardDD, _hmID2Var).launchViewer();
				System.out.println("Press key");
				System.in.read();
			}

		} catch (Exception e) {
			e.printStackTrace();
		}

		
	}
	



	public ArrayList<Integer> convertAddExpr2ADD(EXPR e, boolean filter_actions) throws Exception {
		
		ArrayList<Integer> adds = new ArrayList<Integer>();
		ArrayList<Pair> exprs = getAdditiveComponents(e);
		
//		System.out.println("\n");
		for (Pair p : exprs) {
			String str = "";
			if (p._o2 instanceof RDDL.OPER_EXPR)
				str = ((RDDL.OPER_EXPR)p._o2)._op;
//			System.out.println("Found pair: " + p._o1 + " -- " + p._o2.getClass() + " / " + str + "\n" + p);
		}
		
		int ZERO_ADD = _context.getConstantNode(0d); 
		
		for (Pair p : exprs) {
			
			HashSet<Pair> relevant_vars = new HashSet<Pair>();
			HashMap subs = (HashMap)p._o1;
			EXPR e2 = ((EXPR)p._o2);
			if (_d._bRewardDeterministic) // collectGFluents expects distribution
				e2 = new DiracDelta(e2);
			else{
//				System.out.println("WARNING: May not convert additive reward correctly... check results.");
			}
			e2.collectGFluents(subs, _state, relevant_vars);
			
//			for( Pair p2 : relevant_vars ){
///				PVAR_NAME pvar = (PVAR_NAME)p2._o1;
//			 	if( State.ACTION == _state.getPVariableType(pvar) ){	
//					_bRewardAction = true;
//				}	
//			} 
			
			int add = enumerateAssignments(new ArrayList<Pair>(relevant_vars), e2, subs, 0);
			if (add != ZERO_ADD)
				adds.add(add);
		}
//		System.out.println("Done processing additive expression");
		
		return adds;
	}
	
	// TODO: Make this recursive (append subs, ...)
	// TODO: Make an expression to additive expression converter to be called in buildCPTs()
	// Returns Pair(HashMap subs, EXPR e)

	
//	private void add2InfluenceMap(String cpt_var, HashSet<Pair> relevant_vars, HashMap<PVAR_NAME, ArrayList<ArrayList<LCONST>>> action_vars) {
//		
//		for( Pair pair : relevant_vars ){
//			PVAR_NAME pvar = (PVAR_NAME)pair._o1;
//			ArrayList<LCONST> term = (ArrayList<LCONST>)pair._o2;
//			
////			System.out.println(pvar.toString() + " " + term + " " + (action_vars.get(pvar)!=null) );
//			
//			if( action_vars.get(pvar) != null ){
//				
//				String actname = CleanFluentName(pvar.toString() + term);
//				
//				if( _hmact2vars.get(actname) == null  ){
//					_hmact2vars.put(actname, new ArrayList<String>());
//				}
//				
//				_hmact2vars.get(actname).add(cpt_var+"'");
//			}
//			
//		}
//
////		System.exit(1);
//		
//	}

	private ArrayList<String> guessOrdering(TreeMap<PVAR_NAME, ArrayList<ArrayList<LCONST>>> state_vars, 
			TreeMap<PVAR_NAME, ArrayList<ArrayList<LCONST>>> observ_vars, TreeMap<PVAR_NAME, ArrayList<ArrayList<LCONST>>> action_vars) throws EvalException {
			
			ArrayList<String> order = new ArrayList<String>();
			
			for (int iter = STATE_ITER; iter <= OBSERV_ITER; iter++) {
				
				TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> src = 
					iter == STATE_ITER ? state_vars : observ_vars;
				
				for (Map.Entry<PVAR_NAME,ArrayList<ArrayList<LCONST>>> e : src.entrySet()) {
					
					PVAR_NAME p = e.getKey();
					ArrayList<ArrayList<LCONST>> assignments = e.getValue();
					
					CPF_DEF cpf = _state._hmCPFs.get(new PVAR_NAME(p.toString() + 
							(iter == STATE_ITER ? "'" : "")));
					
					HashMap<LVAR,LCONST> subs = new HashMap<LVAR,LCONST>();
					for (ArrayList<LCONST> assign : assignments) {
		
						String cpt_var = CleanFluentName(p.toString() + assign);
//						System.out.println("Processing: " + cpt_var);
						
		
						subs.clear();
						for (int i = 0; i < cpf._exprVarName._alTerms.size(); i++) {
							LVAR v = (LVAR)cpf._exprVarName._alTerms.get(i);
							LCONST c = (LCONST)assign.get(i);
							subs.put(v,c);
						}
						
						// This method determines all ground fluents relevant to 
						// the cpf for a variable for the given substitution 'subs'
						EXPR cpf_expr = cpf._exprEquals;
						
						HashSet<Pair> relvars = new HashSet<Pair>();
						cpf_expr.collectGFluents(subs, _state, relvars);
						
						for( Pair pa : relvars ){
							PVAR_NAME pvar = (PVAR_NAME)pa._o1;
							ArrayList<LCONST> ass = (ArrayList<LCONST>)pa._o2;
							String name = CleanFluentName(pvar.toString()+ass);
							
							if( !order.contains(name) && _state.getPVariableType(pvar) == _state.STATE ){
								order.add(name);	
							}
							
						}
						
						for( Pair pa : relvars ){
							PVAR_NAME pvar = (PVAR_NAME)pa._o1;
							ArrayList<LCONST> ass = (ArrayList<LCONST>)pa._o2;
							String name = CleanFluentName(pvar.toString()+ass);
							
							if( !order.contains(name) && _state.getPVariableType(pvar) == _state.ACTION ){
								order.add(name);	
							}
							
						}
						
//						System.out.println(relvars);
						order.add(cpt_var+"'");
						
//						System.out.println(order);
					}
				}
			}
			
			if( order.size() != 2*_alstatevars.size() + _alactionvars.size() + _alObservvars.size() ) {
				System.err.println("size of ordering not equal to size of variables");
				System.out.println(order);
				System.out.println(_alstatevars);
				System.out.println(_alactionvars);
				System.out.println(_alObservvars);
				
				System.exit(1);
			}
			
			if( DEBUG0 ) {
				System.out.println("Problem has " + order.size() + " variables ");
				System.out.println("State vars : " + _alstatevars.size());
				System.out.println("Action vars : " + _alactionvars.size());
				System.out.println("Ordering : " + order);	
			}
			
			
			return order;
		}
	
	
private ArrayList<String> makeOrdering() {
		
//		System.out.println(_hmStateVars.keySet());
//		System.out.println(_hmActionMap.keySet());
//		System.out.println(_hmNextStateVars.keySet());
//		
//		
		ArrayList<String> order = new ArrayList<String>();
//		
//		String[] ord = {"person_in_elevator_going_up__e0", "person_in_elevator_going_down__e0", 
//				"elevator_closed__e0", 
//				"elevator_at_floor__e0_f0", "elevator_dir_up__e0", "elevator_at_floor__e0_f1", "elevator_at_floor__e0_f2",
//				"person_waiting_up__f0", "person_waiting_up__f1", "person_waiting_up__f2", 
//				"person_waiting_down__f2", "person_waiting_down__f0", "person_waiting_down__f1"}; 
//		
//		return order;
		
		switch(_order){
		
			case ORDER_INTERLEAVE:
				order.addAll(_alactionvars);
				
				for( String s : _alstatevars ) {
					order.add(s);
					order.add(s+"'");
				}
				
				break;
				
			case ORDER_AXPX:
				order.addAll(_alactionvars);
				order.addAll(_alnextstatevars);
				order.addAll(_alstatevars);
				
				break;
				
			case ORDER_AXXP:
				order.addAll(_alactionvars);
				order.addAll(_alstatevars);
				order.addAll(_alnextstatevars);
				break;
			
			case ORDER_XAXP:
				
				order.addAll(_alstatevars);
				order.addAll(_alactionvars);
				order.addAll(_alnextstatevars);
				break;
				
			case ORDER_XPAX:
				
				order.addAll(_alnextstatevars);
				order.addAll(_alactionvars);
				order.addAll(_alstatevars);
				break;
				
			case ORDER_XPXA:
				
				order.addAll(_alnextstatevars);
				order.addAll(_alstatevars);
				order.addAll(_alactionvars);
				break;
			
			default:
				System.err.println("Unknown ordering...");
			
			case ORDER_XXPA:

				
				order.addAll(_alstatevars);
				order.addAll(_alnextstatevars);
				order.addAll(_alactionvars);
		}
		
		if( DEBUG0 ) {
			System.out.println("Problem has " + order.size() + " variables");
			System.out.println("Ordering : " + order);
		}
		
		return order;
	}
	
	

	
	public TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> collectStateVars() 
	throws EvalException {
	
	TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> state_vars = 
		new TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>>();

	for (PVAR_NAME p : _state._alStateNames) {
		ArrayList<ArrayList<LCONST>> gfluents = _state.generateAtoms(p);
		state_vars.put(p, gfluents);
	}
	
	return state_vars;
}
	

public TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> collectActionVars() 
	throws EvalException {
	
	TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> action_vars = 
		new TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>>();

	for (PVAR_NAME p : _state._alActionNames) {
		ArrayList<ArrayList<LCONST>> gfluents = _state.generateAtoms(p);
		action_vars.put(p, gfluents);
	}
	
	return action_vars;
}

public TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> collectObservationVars() 
	throws EvalException {

	TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> observ_vars = 
		new TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>>();

	for (PVAR_NAME p : _state._alObservNames) {
		ArrayList<ArrayList<LCONST>> gfluents = _state.generateAtoms(p);
		observ_vars.put(p, gfluents);
	}
	
	return observ_vars;
}

public static String CleanFluentName(String s) {
	s = s.replace("[", "__");
	s = s.replace("]", "");
	s = s.replace(", ", "_");
	s = s.replace(',','_');
	s = s.replace(' ','_');
	s = s.replace('-','_');
	s = s.replace("()","");
	s = s.replace("(", "__");
	s = s.replace(")", "");
	if (s.endsWith("__"))
		s = s.substring(0, s.length() - 2);
	return s;
}

public int enumerateAssignments(ArrayList<Pair> vars, 
		EXPR cpf_expr, HashMap<LVAR,LCONST> subs, int index) 
	throws Exception {
	return enumerateAssignments(vars, cpf_expr, subs, index, _context.getConstantNode(1d));
}

public int enumerateAssignments(ArrayList<Pair> vars, 
		EXPR cpf_expr, HashMap<LVAR,LCONST> subs, int index, int cpt) 
	throws EvalException, Exception {

	// Need to build an assignment ADD on the way down, use it 
	// at leaf to add to current CPT and return result
	
	// Recurse until index == vars.size
	if (index >= vars.size()) {
		
		// At this point, all state and action variables are set
		// Just need to get the distribution for the appropriate CPT
		RDDL.EXPR e = cpf_expr.getDist(subs, _state);
//		System.out.println( _state + " " + e );
		double prob_true = -1d;
		//System.out.println("RDDL.EXPR: " + e);
		if (e instanceof KronDelta) {
			EXPR e2 = ((KronDelta)e)._exprIntValue;
			if (e2 instanceof INT_CONST_EXPR)
				// Should either be true (1) or false (0)... same as prob_true
				prob_true = (double)((INT_CONST_EXPR)e2)._nValue;
			else if (e2 instanceof BOOL_CONST_EXPR)
				prob_true = ((BOOL_CONST_EXPR)e2)._bValue ? 1d : 0d;
			else
				throw new EvalException("Unhandled KronDelta argument: " + e2.getClass()); 
		} else if (e instanceof Bernoulli) {
			prob_true = ((REAL_CONST_EXPR)((Bernoulli)e)._exprProb)._dValue;
		} else if (e instanceof DiracDelta) {
			// NOTE: this is not a probability, but rather an actual value
			//       (presumably for the reward).  This method is a little
			//       overloaded... need to consider whether there will be
			//       any problems arising from this overloading.  -Scott
			prob_true = ((REAL_CONST_EXPR)((DiracDelta)e)._exprRealValue)._dValue;
		} else
			throw new EvalException("Unhandled distribution type: " + e.getClass());
		
//		System.out.println(prob_true);
		
		// Now build CPT for action variables
		return _context.scalarMultiply(cpt, prob_true);

	} else {
//		System.out.println(vars + " " + index);
		
		PVAR_NAME p = (PVAR_NAME)vars.get(index)._o1;
		ArrayList<LCONST> terms = (ArrayList<LCONST>)vars.get(index)._o2;
		String var_name = CleanFluentName(p._sPVarName + terms + (p._bPrimed ? "\'" : ""));
//		System.out.println(var_name);
//		System.out.println(_hmVar2ID);
		
		int varid = _hmVar2ID.get(var_name);
		

		// Set to true
		_state.setPVariableAssign(p, terms, RDDL.BOOL_CONST_EXPR.TRUE);
		int high = _context.getVarNode(varid, 0d, 1d);
		int ret_high = enumerateAssignments(vars, cpf_expr, subs, index + 1, _context.applyInt(cpt, high, DD.ARITH_PROD));

		// Set to false
		_state.setPVariableAssign(p, terms, RDDL.BOOL_CONST_EXPR.FALSE);
		int low  = _context.getVarNode(varid, 1d, 0d);
		int ret_low = enumerateAssignments(vars, cpf_expr, subs, index + 1, _context.applyInt(cpt, low, DD.ARITH_PROD));
		
		// Unassign
		_state.setPVariableAssign(p, terms, null);
		
		int res = _context.applyInt(ret_high, ret_low, DD.ARITH_SUM);
		
		return res;
	}
}
	
 
	public static void main(String[] args) {

		ValueIterationConcurrentAADD vicaadd = null;
		try{
			vicaadd = new ValueIterationConcurrentAADD(args[0], 
				args[1],Double.parseDouble(args[2]), 
				ValueIterationConcurrentAADD.ORDER_GUESS, -1, "./");
		}catch(Exception e){
		}
		System.out.println(vicaadd);
	}

	public String toString(){
		return rInfo.toString() +"\n"+ presolve.toString() +"\n"+ iData.toString() +"\n"+ postsolve.toString();
	}	
}
